#include "robotParser.h"

bool robotParser(string s, unsigned char com, Motor& mr, LED& gn, LED& be, LED& rd, Bumper bump)
{
//	int com;
	char c;
//	com = atoi(s.substr(1, s.size() - 1).c_str());
	c = s[0];
	if(c == '<')
	{
		mr.leftWheelVar((char) com);
	}
	else if(c == '>')
	{
		mr.rightWheelVar((char) com);
	}
	else {
		switch(com)
		{
			case 16:
				rd.setLED(false);
				break;
			case 17:
				rd.setLED(true);
				break;
			case 32:
				gn.setLED(false);
				break;
			case 33:
				gn.setLED(true);
				break;
			case 48:
				be.setLED(false);
				break;
			case 49:
				be.setLED(true);
				break;
			case 64:
				mr.rightWheelStop();
				break;
			case 65:
				mr.rightWheelGo();
				break;
			case 128:
				mr.leftWheelStop();
				break;
			case 129:
				mr.leftWheelGo();
				break;
			case 192:
				mr.stop();
				break;
			case 193:
				mr.go();
				break;
			case 197:
				mr.spinLeft();
				break;
			case 198:
				mr.spinRight();
				break;
			default:
//				cout<< "Invalid peripheral command:  " << c << " " << com << endl;
                return false;
			break;
		}
	}
	return true;
}
